package bcit.nxt.mission;

// TurtleSquare.java
import gin.and.tonic.*;
import lejos.nxt.Button;

public class MissionPower implements UltrasonicListener, TouchListener
{

    private DragonRobot robot = new DragonRobot();
    UltrasonicSensor us = new UltrasonicSensor(SensorPort.S1);
    TouchSensor ts = new TouchSensor(SensorPort.S2);
    LightSensor rightEye = new LightSensor(SensorPort.S4);
    LightSensor leftEye = new LightSensor(SensorPort.S3);
    Motor arm = new Motor(MotorPort.A);
    int v =350;//420;//blue river
    double r = 0.05;
    int spotdistance = 7;
    
    int black=420;
    int backcount=0;

    MissionPower()
    {


        robot.addPart(rightEye);
        robot.addPart(leftEye);
        robot.addPart(us);
        robot.addPart(ts);
        rightEye.activate(true);
        leftEye.activate(true);

        arm.setSpeed(25);
        robot.addPart(arm);

        /*Mission uranium*/
        robot.setTurtleSpeed(60);


        robot.forward(700); //get out of the base
        robot.left(35);
        robot.forward(100);
        arm.rotateTo(-180);
        robot.right(80);
        robot.forward(240);
        robot.right(90);
        spotToRiver();
        robot.backward(100);
        robot.right(30);
        robot.forward(500);



       



        robot.exit();

    }

    private void riverToSpot()
    {
        robot.getGear().forward();
        while (true)
        {
            int leftValue = leftEye.getValue();
            int rightValue = rightEye.getValue();
            
            int distance = us.getDistance();

            if (distance <= spotdistance)
            {
                robot.getGear().stop();
                break;// in front of the wall
            }

            if (leftValue < v && rightValue < v)
            {
                robot.getGear().forward();
                continue;
            }
            if (leftValue < v && rightValue > v)
            {
                robot.getGear().leftArc(r);
                continue;
            }
            if (leftValue > v && rightValue < v)
            {
                robot.getGear().rightArc(r);
                continue;
            }
            if (leftValue > v && rightValue > v)
            {
               //robot.getGear().backward();
                continue;
            }
        }
    }

    private void spotToRiver()
    {
        robot.getGear().forward();
        while (true)
        {
            int leftValue = leftEye.getValue();
            int rightValue = rightEye.getValue();
            
            int distance = us.getDistance();
            distance = us.getDistance();

         

            if (leftValue < v && rightValue < v)
            {
                robot.getGear().forward();
                continue;
            }
            if (leftValue < v && rightValue > v)
            {
                robot.getGear().leftArc(r);
                continue;
            }
            if (leftValue > v && rightValue < v)
            {
                robot.getGear().rightArc(r);
                continue;
            }
            if (leftValue > v && rightValue > v)
            {
               robot.getGear().backward();
              backcount++;
               if(backcount>=3)
                   break;
                continue;
                
            }
        }
    }

    

    public void far(SensorPort port, int level)
    {
    }

    public void near(SensorPort port, int level)
    {
    }

    public void pressed(SensorPort port)
    {
        robot.getGear().stop();
        robot.exit();
    }

    public void released(SensorPort port)
    {
    }

    public static void main(String[] args)
    {

        new MissionPower();
    }
}
